/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "World.h"
#include "Sonar.h"
#include "owndebug.h"
#include "mathutils.h"
#include "drawutils.h"
#include "settings.h"

CSonar::CSonar(CWorld *World, CObject *ParentObject, dBodyID parentBodyID, dReal *posOffset, dMatrix3 rotOffset, 
          float range, float beamAngle, float direction)
:CObject(World, ParentObject),
parentBody(parentBodyID),
range(range),
beamAngle(beamAngle),
direction(direction),
posOffset(posOffset)
{
  for (int i=0; i<12;i++)
    this->rotOffset[i]=rotOffset[i];
  
  raySpaceID=dSimpleSpaceCreate(0);
  
  material.look.color[0]=0.4;material.look.color[1]=0.6;material.look.color[2]=0.1;
  
  Parts.push_back(cylinder=new CCylinder(0.01,0.01,this,this->SpaceID,parentBody,&material,false));
  
  dGeomSetOffsetPosition(cylinder->GeomID,posOffset[0],posOffset[1],posOffset[2]);
  dGeomSetOffsetRotation(cylinder->GeomID,rotOffset);
  
  const dReal *sonarrot=dGeomGetOffsetRotation(cylinder->GeomID);
  
  dPrint(1,"%f %f %f",sonarrot[0],sonarrot[1],sonarrot[2]);
    dPrint(1,"%f %f %f",sonarrot[4],sonarrot[5],sonarrot[6]);
    dPrint(1,"%f %f %f",sonarrot[8],sonarrot[9],sonarrot[10]);
  
  dMatrix3 rot,rot2;
  
  float angle=0;
  for (int i=0;i<NUM_OF_CLUSTERS;i++)
  {
    angle=i*M_PI/NUM_OF_CLUSTERS;
    dRFromAxisAndAngle(rot,0,0,1,angle);
    
    OdeMxM3(rotOffset,rot,rot2);
    
    rays.push_back(new CRayCluster(this, raySpaceID,parentBody,posOffset,rot2, 
        NUM_OF_RAYS_PER_CLUSTER,range, -beamAngle/2.0,beamAngle/2.0));
    Parts.push_back( rays.back() );
    //rays.back()->IsVisible=true;
  }
  
}

void CSonar::Activate()
{
  
}

void CSonar::Draw()
{
  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++)
  {
    (*i)->Draw();
  }
  
}
    
void CSonar::Update(float StepSize)
{
  
}
    
MaCI::Ranging::TDistance CSonar::GetReading()
{
  MaCI::Ranging::TDistance d;
  
  distance=-1;
  
  for (std::vector<CRayCluster*>::iterator i=rays.begin();i!=rays.end();i++)
  {
      (*i)->Set();
  }
  dSpaceCollide2((dGeomID)raySpaceID,(dGeomID)SpaceID,this,&RayCallback);
  
  d.angle=direction;
  d.distance=distance;
  return d;
}

void CSonar::RayCallback(void *data, dGeomID o1, dGeomID o2)
{
  CSonar *sonar=(CSonar*) data;

  float angle=0;
  dVector3 rayDir, rayStart;
  
    
  if (o1 == sonar->cylinder->GeomID || o2 == sonar->cylinder->GeomID) //dont take readings from the casing
    return;
  
  if (dGeomGetClass(o1)== dRayClass)
  {
    dGeomRayGet(o1,rayStart,rayDir);
  } else if (dGeomGetClass(o2)== dRayClass){
    dGeomRayGet(o2,rayStart,rayDir);
  } else {
    return;
  }
  
  dContact contact;
  if (dCollide(o1, o2, 1, &contact.geom, sizeof(dContact)))
  {
    
    //test if angle between surface and ray is small enough
    //Note: raydir and contact normal are unit vectors so its ok to take acos 
    //from dot product directly
    angle=acos(DotProduct(rayDir,contact.geom.normal));
    
    //dPrint(1,"angle=%f",RadToDeg(angle));
    
    /*IMPORTANT!: ray collision detection returns many collisions, but not on same callback call
    so remember to choose the shortest distance value!!
    */
    if (angle <=ECHO_TRESHOLD_ANGLE && (sonar->distance<0 || contact.geom.depth < sonar->distance))
    {
      sonar->distance=contact.geom.depth;
    }  
  }
}

void CSonar::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{

}
